#include "src/scientific_computing/linear_algebra/solvesystems.h"

double** gaussinv(double** mat, int n)
{
  double** res = NULL;

  // locals
  double** DAP = NULL;
  int* IP = NULL;
  int lnl = 0, lnc = 0;

  int m = n*2;
  double dvmax = 0.0;
  double tmp;
  int tmpint;

#ifndef HAVE_MDMALLOC
  GN_MALLOC(DAP, double*, n, "gaussinv::DAP_l");
#endif
#ifdef HAVE_MDMALLOC
  DAP = (double**)vmdmalloc(sizeof(double), 2, n, m);
  memset( &(DAP[0][0]),0, sizeof(double)*n*m );
#endif
  for (int i = 0; i < n; ++i)
    {
#ifndef HAVE_MDMALLOC
      GN_CALLOC(DAP[i], double, m, "gaussinv::DAP_c");
#endif
      for (int j = 0; j < n; ++j)
        {
          DAP[i][j] = mat[i][j];
        }
    }
#ifndef HAVE_MDMALLOC
  GN_MALLOC(IP, int, n, "gaussinv::IP");
#endif
#ifdef HAVE_MDMALLOC
  IP = (int*)vmdmalloc(sizeof(int), 1, n);
#endif
  for (int i = 0; i < n; ++i)
    {
      DAP[i][n + i] = 1.0;
      IP[i] = i;
    }

  //recherche de l element maximum
  for (int i = 0; i < n - 1; ++i)
    {
      dvmax = 0.0;
      lnl = i;
      lnc = i;
      for (int j = i; j < n; ++j)
        {
          for (int k = i; k < n; ++k)
            {
              if (fabs(DAP[j][k]) > dvmax)
                {
                  dvmax = fabs(DAP[j][k]);
                  lnl = j;
                  lnc = k;
                }
            }
        }
      //permutation ligne I/LNL
      for (int j = 0; j < m; ++j)
        {
          tmp = DAP[i][j];
          DAP[i][j] = DAP[lnl][j];
          DAP[lnl][j] = tmp;
        }
      //permutation colonne J/LNC
      for (int j = 0; j < n; ++j)
        {
          tmp = DAP[j][i];
          DAP[j][i] = DAP[j][lnc];
          DAP[j][lnc] = tmp;
        }
      tmpint = IP[i];
      IP[i] = IP[lnc];
      IP[lnc] = tmpint;
    }

  // GAUSS
  // elimination de la matrice
  // balayage vers le bas
  for (int i = 0; i < n - 1; ++i)
    {
      for (int k = i + 1; k < n; ++k)
        {
          for (int j = m - 1; j >= i; --j)
            {
              DAP[k][j] -= (DAP[k][i] / DAP[i][i]) * DAP[i][j];
            }
        }
    }
  // balayage vers le haut
  for (int i = n - 1; i >= 0; --i)
    {
      for (int j = m - 1; j >= i; --j)
        {
          DAP[i][j] /= DAP[i][i];
        }
      for (int k = i - 1; k >= 0; --k)
        {
          for (int j = m - 1; j >= i; --j)
            {
              DAP[k][j] -= DAP[k][i] * DAP[i][j];
            }
        }
    }

#ifndef HAVE_MDMALLOC
  GN_CALLOC(res, double*, n, "gaussinv::res_l");
  for (int i = 0; i < n; ++i)
    {
      GN_CALLOC(res[i], double, n, "gaussinv::res_c");
    }
#endif
#ifdef HAVE_MDMALLOC
  res = (double**)vmdmalloc(sizeof(double), 2, n, n);
  memset( &(res[0][0]),0, sizeof(double)*n*n );
#endif

  for (int i = 0; i < n; ++i)
    {
      for (int j = 0; j < n; ++j)
        {
          res[IP[i]][j] = DAP[i][j + n];
        }
    }
  // cleanup
  free_and_null(IP);
#ifndef HAVE_MDMALLOC
  for (int i = 0; i < n; ++i)
    {
      free(DAP[i]);
    }
  free_and_null(DAP);
#endif
#ifdef HAVE_MDMALLOC
  free(DAP);
#endif
  return res;
}

double* matvect(double** & A, double* & x, int n)
{
  double* res = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(res, double, n, "matvect::res");
#endif
#ifdef HAVE_MDMALLOC
  res = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(res[0]),0, sizeof(double)*n );
#endif

  for (int i=0; i< n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          res[i]+=A[i][j]*x[j];
        }
    }
  return res;
}

double* matTvect(double** & A, double* & x, int m, int n)
{
  double* res = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(res, double, n, "matTvect::res");
#endif
#ifdef HAVE_MDMALLOC
  res = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(res[0]),0, sizeof(double)*n );
#endif

  for (int i=0; i< n; ++i)
    {
      for (int j=0; j<m; ++j)
        {
          res[i]+=A[j][i]*x[j];
        }
    }
  return res;
}

double** matmat(double** & A, double** & B, int n)
{
  double** res = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(res, double*, n, "matmat::res_l");
  for (int i=0; i<n; ++i)
    {
      GN_CALLOC(res[i], double, n, "matmat::res_c");
    }
#endif
#ifdef HAVE_MDMALLOC
  res = (double**)vmdmalloc(sizeof(double), 2, n, n);
  memset( &(res[0][0]),0, sizeof(double)*n*n );
#endif

  for (int i=0; i< n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          for (int k=0; k<n; ++k)
            {
              res[i][j]+=A[i][k]*B[k][j];
            }
        }
    }
  return res;
}

double** matTmat(double** & A, double** & B, int m, int n)
{
  double** res = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(res, double*, n, "matmat::res_l");
  for (int i=0; i<n; ++i)
    {
      GN_CALLOC(res[i], double, n, "matmat::res_c");
    }
#endif
#ifdef HAVE_MDMALLOC
  res = (double**)vmdmalloc(sizeof(double), 2, n, n);
  memset( &(res[0][0]),0, sizeof(double)*n*n );
#endif

  for (int i=0; i< n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          for (int k=0; k<m; ++k)
            {
              res[i][j]+=A[k][i]*B[k][j];
            }
        }
    }
  return res;
}

double** matprepost(double** & A, double** & B, int n)
{
  double** res = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(res, double*, n, "matprepost::res_l");
  for (int i=0; i<n; ++i)
    {
      GN_CALLOC(res[i], double, n, "matprepost::res_c");
    }
#endif
#ifdef HAVE_MDMALLOC
  res = (double**)vmdmalloc(sizeof(double), 2, n, n);
  memset( &(res[0][0]),0, sizeof(double)*n*n );
#endif

  for (int i=0; i< n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          for (int k=0; k<n; ++k)
            {
              for (int l=0; l<n; ++l)
                {
                  res[i][j]+=A[i][k]*B[k][l]*A[l][j];
                }
            }
        }
    }
  return res;
}

double** matdiag(double** & A, double* & D, double** & B, int n)
{
  //compute ADB
  double** res = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(res, double*, n, "matdiag::res_l");
  for (int i=0; i<n; ++i)
    {
      GN_CALLOC(res[i], double, n, "matdiag::res_c");
    }
#endif
#ifdef HAVE_MDMALLOC
  res = (double**)vmdmalloc(sizeof(double), 2, n, n);
  memset( &(res[0][0]),0, sizeof(double)*n*n );
#endif

  for (int i=0; i< n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          for (int k=0; k<n; ++k)
            {
              res[i][j]+=A[i][k]*D[k]*B[k][j];
            }
        }
    }
  return res;
}

double** matdiagtranspose(double** & A, double* & D, double** & B, int n)
{
  // Compute ADB^t
  double** res = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(res, double*, n, "matdiag::res_l");
  for (int i=0; i<n; ++i)
    {
      GN_CALLOC(res[i], double, n, "matdiag::res_c");
    }
#endif
#ifdef HAVE_MDMALLOC
  res = (double**)vmdmalloc(sizeof(double), 2, n, n);
  memset( &(res[0][0]),0, sizeof(double)*n*n );
#endif

  for (int i=0; i< n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          for (int k=0; k<n; ++k)
            {
              res[i][j]+=A[i][k]*D[k]*B[j][k];
            }
        }
    }
  return res;
}


double* zrg_rt(double** & A, double* & b, int n)
{
  double* xk = NULL;

  // locals
  double* R = NULL;
  double alpha = 1.0; // should be smaller than 2/rho(RA)
  double* Rb=NULL;

  bool done=false;
  double test;
  int k_iter=0;

  // constants
  static int max_iter=100000;
  static double rt_eps = 1E-6;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(R, double, n, "zrg_rt::R");
  GN_CALLOC(xk,  double, n, "zrg_rt::xk");
  GN_CALLOC(Rb, double, n, "zrg_rt::Rb");
#endif
#ifdef HAVE_MDMALLOC
  R  = (double*)vmdmalloc(sizeof(double), 1, n);
  Rb  = (double*)vmdmalloc(sizeof(double), 1, n);
  xk = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(R[0]),0, sizeof(double)*n );
  memset( &(Rb[0]),0, sizeof(double)*n );
  memset( &(xk[0]),0, sizeof(double)*n );
#endif

  for (int i=0; i < n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          R[i] += fabs(A[i][j]);
        }
      R[i] = 1.0/R[i];
      Rb[i] = R[i]*b[i];
    }

  while (!done)
    {
      // x_{k+1} = x_k + \alpha R( b - Ax_k )
      double* tmp = matvect(A, xk, n);
      test=0.0;
      for (int i=0; i< n; ++i)
        {
          xk[i] = xk[i] + alpha*(Rb[i] - R[i]*tmp[i]);
          test += pow((b[i] - tmp[i]),2.0);
        }
      free(tmp);

#ifdef RETD_DEBUG
      std::cout << "ZRG_RT iteration="<<k_iter<<"\n";
      for (int i=0; i<n; ++i)
        {
          std::cout << "xk["<<i<<"]="<<xk[i]<<"\n";
        }
#endif

      // stopping condition
      done = ( ( sqrt(test) < rt_eps ) || (k_iter > max_iter) );
      ++k_iter;
    }
  if (k_iter > max_iter)
    {
      std::cout << "ZRG_RT did not converge\n";
      std::cout << "norm="<<test<<"\n";
    }

  // free stuff
  free(R);
  free(Rb);

  return xk;
}


double* zrg_shb(double** & A, double* & b, int n)
{
  double* res = NULL;

  // locals
  double** Rk = NULL;

  bool done=false;
  double test;
  int k_iter=0;

  // constants
  static int max_iter=1000;
  static double rt_eps = 1E-6;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(Rk, double*, n, "zrg_shb::Rk_l");
  for (int i=0; i < n; ++i)
    {
      GN_CALLOC(Rk[i], double, n, "zrg_shb::Rk_c");
    }
  GN_CALLOC(res, double, n, "zrg_shb::res");
#endif
#ifdef HAVE_MDMALLOC
  Rk  = (double**)vmdmalloc(sizeof(double), 2, n, n);
  res = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(Rk[0][0]),0, sizeof(double)*n*n );
  memset( &(res[0]),0, sizeof(double)*n );
#endif

  for (int i=0; i < n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          Rk[i][i] += fabs(A[i][j]);
        }
      Rk[i][i] = 1.0/Rk[i][i];
    }

  while (!done)
    {
      // R_{k+1} = 2R_k - R_kAR_k
      double** tmp = matprepost(Rk, A, n);
      for (int i=0; i< n; ++i)
        {
          res[i]=0.0;
          for (int j=0; j<n; ++j)
            {
              Rk[i][j] = 2*Rk[i][j] - tmp[i][j];
              res[i]+=Rk[i][j]*b[j];
            }
        }
      test=0.0;
      for (int i=0; i< n; ++i)
        {
          double myEl=0.0;
          for (int j=0; j<n; ++j)
            {
              myEl += A[i][j]*res[j];
            }
          test += pow((b[i] - myEl),2.0);
        }
#ifndef HAVE_MDMALLOC
      for (int i=0; i<n; ++i)
        {
          free(tmp[i]);
        }
#endif
      free(tmp);

#ifdef RETD_DEBUG
      std::cout << "ZRG_SHB iteration="<<k_iter<<"\n";
      for (int i=0; i<n; ++i)
        {
          std::cout << "xk["<<i<<"]="<<res[i]<<"\n";
        }
#endif

      // stopping condition
      done = ( ( sqrt(test) < rt_eps ) || (k_iter > max_iter) );
      ++k_iter;
    }
  if (k_iter > max_iter)
    {
      std::cout << "ZRG_SHB did not converge\n";
      std::cout << "norm="<<test<<"\n";
    }

  // free stuff
#ifndef HAVE_MDMALLOC
  for (int i=0; i<n; ++i)
    {
      free(Rk[i]);
    }
#endif
  free(Rk);

  return res;
}

double sign(double a, double b)
{
  if ( b > 0)
    {
      return fabs(a);
    }
  else
    {
      return -fabs(a);
    }
}

double pythag(double a, double b)
{
  double absa,absb;
  absa=fabs(a);
  absb=fabs(b);
  if (absa > absb)
    {
      return absa*sqrt(1.0+pow(absb/absa,2.0));
    }
  else
    {
      return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+pow(absa/absb,2.0)));
    }
}

void QRdcmp(double** const A, int n, double** & R, double* & c, double* & d, bool sing)
{
  // Decomposes A and returns R, where d is the diagonal of R

  // Locals
  double scale, sigma, sum, tau;

  for (int i = 0; i < n; ++i)
    {
      for (int j = 0; j < n; ++j)
        {
          R[i][j] = A[i][j];
        }
    }

  sing = false;
  for (int k=0; k < n - 1; ++k)
    {
      scale = 0.0;
      for (int i=k; i < n; ++i)
        {
          scale = (std::max)(scale, fabs(R[i][k]));
        }
      if ( scale == 0.0)
        {
          sing=true;
          c[k] = 0.0;
          d[k] = 0.0;
        }
      else
        {
          for (int i=k; i < n; ++i)
            {
              R[i][k] /= scale;
            }
          sum=0.0;
          for (int i=k; i < n; ++i)
            {
              sum += pow(R[i][k],2.0);
            }
          sigma = sign(sqrt(sum), R[k][k]);
          R[k][k] += sigma;
          c[k] = sigma*R[k][k];
          d[k] = -scale*sigma;
          for (int j=k+1; j < n; ++j)
            {
              sum =0.0;
              for (int i=k; i < n; ++i)
                {
                  sum += R[i][k]*R[i][j];
                }
              tau = sum/c[k];
              for (int i=k; i < n; ++i)
                {
                  R[i][j] -= tau*R[i][k];
                }
            }
        }
    }
  d[n-1] = R[n-1][n-1];
  if (d[n-1]==0.0)
    {
      sing = true;
    }
}

double* QRsolve(double** const A, double* const b, int n)
{
  // Solves Ax = b, using a QR algorithm
  double* x;
  double sum, tau;

  // locals
  double** R = NULL;
  double* c = NULL;
  double* d = NULL;
  bool is_sing=false;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(R, double*, n, "QRsolve::R_l");
  for (int i=0; i < n; ++i)
    {
      GN_CALLOC(R[i], double, n, "QRsolve::R_c");
    }
  GN_CALLOC(c, double, n, "QRsolve::c");
  GN_CALLOC(d, double, n, "QRsolve::d");
  GN_CALLOC(x, double, n, "QRsolve::x");
#endif
#ifdef HAVE_MDMALLOC
  R  = (double**)vmdmalloc(sizeof(double), 2, n, n);
  x  = (double*)vmdmalloc(sizeof(double), 1, n);
  c  = (double*)vmdmalloc(sizeof(double), 1, n);
  d  = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(R[0][0]),0, sizeof(double)*n*n );
  memset( &(x[0]),0, sizeof(double)*n );
  memset( &(c[0]),0, sizeof(double)*n );
  memset( &(d[0]),0, sizeof(double)*n );
#endif

  // copy de b dans x
  for (int i=0; i < n; ++i)
    {
      x[i] = b[i];
    }

  QRdcmp(A, n, R, c, d, is_sing);
#ifdef RETD_DEBUG
  std::cout << "is_sing "<<is_sing << "\n";
  for (int i=0; i<n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          std::cout << " R["<<i<<"]["<<j<<"]="<<R[i][j];
        }
      std::cout << "\n";
      std::cout << "d["<<i<<"]="<<d[i]<<"\n";
      std::cout << "c["<<i<<"]="<<c[i]<<"\n";
    }
#endif

  if (is_sing)
    {
      std::cout << "QRsolve : System is Singular, couldn't Solve it !\n";
    }
  else
    {
      for (int j=0; j < n - 1; ++j)
        {
          sum=0.0;
          for (int i=j; i < n; ++i)
            {
              sum += R[i][j]*x[i];
            }
          tau = sum/c[j];
          for (int i=j; i < n; ++i)
            {
              x[i] -= tau*R[i][j];
            }
        }
      // Back Substitution
      x[n-1] /= d[n-1];
      for (int i=n-2; i>=0; --i)
        {
          sum=0.0;
          for (int j=i+1; j < n; ++j)
            {
              sum += R[i][j]*x[j];
            }
          x[i] = (x[i]-sum)/d[i];
        }
    }

  // Free
#ifndef HAVE_MDMALLOC
  for (int i=0; i<n; ++i)
    {
      free(R[i]);
    }
#endif
  free(R);
  free(c);
  free(d);

  return x;
}

double* QRsolve(double* b, int n, double** const R, double* const c, double* const d, bool is_sing)
{
  // Solves Ax = b, using a QR algorithm, decomposition should already be done ...
  double* x;
  double sum, tau;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(x, double, n, "QRsolve::x");
#endif
#ifdef HAVE_MDMALLOC
  x  = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(x[0]),0, sizeof(double)*n );
#endif

  // copy de b dans x
  for (int i=0; i < n; ++i)
    {
      x[i] = b[i];
    }

#ifdef RETD_DEBUG
  std::cout << "is_sing "<<is_sing << "\n";
  for (int i=0; i<n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          std::cout << " R["<<i<<"]["<<j<<"]="<<R[i][j];
        }
      std::cout << "\n";
      std::cout << "d["<<i<<"]="<<d[i]<<"\n";
      std::cout << "c["<<i<<"]="<<c[i]<<"\n";
    }
#endif

  if (is_sing)
    {
      std::cout << "QRsolve : System is Singular, couldn't Solve it !\n";
    }
  else
    {
      for (int j=0; j < n - 1; ++j)
        {
          sum=0.0;
          for (int i=j; i < n; ++i)
            {
              sum += R[i][j]*x[i];
            }
          tau = sum/c[j];
          for (int i=j; i < n; ++i)
            {
              x[i] -= tau*R[i][j];
            }
        }
      // Back Substitution
      x[n-1] /= d[n-1];
      for (int i=n-2; i>=0; --i)
        {
          sum=0.0;
          for (int j=i+1; j < n; ++j)
            {
              sum += R[i][j]*x[j];
            }
          x[i] = (x[i]-sum)/d[i];
        }
    }

  return x;
}


void QRsolve(double** & A, double* & b, int n, double* & x)
{
  // Solves Ax = b, using a QR algorithm
  // x doit etre deja alloue
  double sum, tau;

  // locals
  double** R = NULL;
  double* c = NULL;
  double* d = NULL;
  bool is_sing=false;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(R, double*, n, "QRsolve::R_l");
  for (int i=0; i < n; ++i)
    {
      GN_CALLOC(R[i], double, n, "QRsolve::R_c");
    }
  GN_CALLOC(c, double, n, "QRsolve::c");
  GN_CALLOC(d, double, n, "QRsolve::d");
#endif
#ifdef HAVE_MDMALLOC
  R  = (double**)vmdmalloc(sizeof(double), 2, n, n);
  c  = (double*)vmdmalloc(sizeof(double), 1, n);
  d  = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(R[0][0]),0, sizeof(double)*n*n );
  memset( &(c[0]),0, sizeof(double)*n );
  memset( &(d[0]),0, sizeof(double)*n );
#endif

  // copy de b dans x
  for (int i=0; i < n; ++i)
    {
      x[i] = b[i];
    }

  QRdcmp(A, n, R, c, d, is_sing);
#ifdef RETD_DEBUG
  std::cout << "is_sing "<<is_sing << "\n";
  for (int i=0; i<n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          std::cout << " R["<<i<<"]["<<j<<"]="<<R[i][j];
        }
      std::cout << "\n";
      std::cout << "d["<<i<<"]="<<d[i]<<"\n";
      std::cout << "c["<<i<<"]="<<c[i]<<"\n";
    }
#endif

  if (is_sing)
    {
      std::cout << "QRsolve : System is Singular, couldn't Solve it !\n";
    }
  else
    {
      for (int j=0; j < n - 1; ++j)
        {
          sum=0.0;
          for (int i=j; i < n; ++i)
            {
              sum += R[i][j]*x[i];
            }
          tau = sum/c[j];
          for (int i=j; i < n; ++i)
            {
              x[i] -= tau*R[i][j];
            }
        }
      // Back Substitution
      x[n-1] /= d[n-1];
      for (int i=n-2; i>=0; --i)
        {
          sum=0.0;
          for (int j=i+1; j < n; ++j)
            {
              sum += R[i][j]*x[j];
            }
          x[i] = (x[i]-sum)/d[i];
        }
    }

  // Free
#ifndef HAVE_MDMALLOC
  for (int i=0; i<n; ++i)
    {
      free(R[i]);
    }
#endif
  free(R);
  free(c);
  free(d);
}

// Lu stuff

void LUdcmp(double** & A, double** & U, int n, int* & indx, int d)
{

  //locals
  double* vv;
  double big, dum, sum;
  int imax;

  // some Alloc Stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(vv, double, n, "LUdcmp::vv");
#endif
#ifdef HAVE_MDMALLOC
  vv = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(vv[0]),0, sizeof(double)*n );
#endif

  // Copy of A into U
  for (int i = 0; i < n; ++i)
    {
      for (int j = 0; j < n; ++j)
        {
          U[i][j] = A[i][j];
        }
    }

  d = 1;
  for (int i=0; i < n; ++i)
    {
      big=0.0;
      for (int j=0; j < n; ++j)
        {
          big = (std::max)(fabs(U[i][j]),big);
        }
      if ( big == 0.0 )
        {
          std::cout << "LUdcmp : System is Singular !\n";
          exit(false);
        }
      vv[i] = 1.0/big;
    }
  for (int j=0; j <n ; ++j)
    {
      for (int i=0; i < j; ++i)
        {
          sum = U[i][j];
          for (int k=0; k<i; ++k)
            {
              sum -= U[i][k]*U[k][j];
            }
          U[i][j]=sum;
        }
      big = 0.0;
      for (int i=j; i <n; ++i)
        {
          sum = U[i][j];
          for (int k=0; k < j; ++k)
            {
              sum -= U[i][k]*U[k][j];
            }
          U[i][j] = sum;
          dum = vv[i]*fabs(sum);
          if (dum >= big)
            {
              big = dum;
              imax = i;
            }
        }
      if (j != imax)
        {
          // row permutation
          for (int k=0; k <n; ++k)
            {
              dum = U[imax][k];
              U[imax][k] = U[j][k];
              U[j][k] = dum;
            }
          d *= -1;
          vv[imax]=vv[j];
        }
      indx[j] = imax;
      // Only if we want algorithm to work on singular matrices....
      //if ( U[j][j] = 0.0){
      //	U[j][j] = TINY;
      //}
      if ( 	j != n)
        {
          dum = 1.0/U[j][j];
          for (int i=j+1; i<n; ++i)
            {
              U[i][j] *= dum;
            }
        }
    }
  free_and_null(vv);
}

double* LUsolve(double** & A, double* & b, int n)
{
  // Solves Ax = b, using a LU algorithm
  double* x;

  // locals
  double** U = NULL;
  int* indx = NULL;
  int d=0;

  int ip;
  int ii=-1;
  double sum;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(U, double*, n, "LUsolve::U_l");
  for (int i=0; i < n; ++i)
    {
      GN_CALLOC(U[i], double, n, "LUsolve::U_c");
    }
  GN_CALLOC(indx, int, n, "LUsolve::indx");
  GN_CALLOC(x, double, n, "LUsolve::x");
#endif
#ifdef HAVE_MDMALLOC
  U     = (double**)vmdmalloc(sizeof(double), 2, n, n);
  x     = (double*)vmdmalloc(sizeof(double), 1, n);
  indx  = (int*)vmdmalloc(sizeof(int), 1, n);
  memset( &(U[0][0]),0, sizeof(double)*n*n );
  memset( &(x[0]),0, sizeof(double)*n );
  memset( &(indx[0]),0, sizeof(int)*n );
#endif

  // copy de b dans x
  for (int i=0; i < n; ++i)
    {
      x[i] = b[i];
    }

  LUdcmp(A, U, n, indx, d);
#ifdef RETD_DEBUG
  for (int i=0; i<n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          std::cout << " U["<<i<<"]["<<j<<"]="<<U[i][j];
        }
      std::cout << "\n";
      std::cout << "indx["<<i<<"]="<<indx[i]<<"\n";
    }
#endif

  for (int i=0; i < n; ++i)
    {
      ip = indx[i];
      sum = x[ip];
      x[ip]=x[i];
      if (ii>=0)
        {
          for (int j=ii; j < i; ++j)
            {
              sum -= U[i][j]*x[j];
            }
        }
      else if (sum != 0.0)
        {
          ii=i;
        }
      x[i] = sum;
    }
  for (int i=n-1; i>=0; --i)
    {
      sum = x[i];
      for (int j=i+1; j<n; ++j)
        {
          sum -= U[i][j]*x[j];
        }
      x[i] = sum/U[i][i];
    }

  // Free stuff
#ifndef HAVE_MDMALLOC
  for (int i=0; i<n; ++i)
    {
      free(U[i]);
    }
#endif
  free(U);
  free(indx);

  return x;
}

void SVDdcmp(double** & A, int m,int n , double** & U, double* & w, double** & V)
{
  /*
  Given a matrix A m x n, computes the SVD, A = U W V^T,
  where W is output as a vector, as it is a diagonal matrix
  */
  //locals
  double* tmp;

  // more locals
  bool flag;
  int itmax=30;
  int l,nm;
  double anorm, c,f,g,h,s,scale,x,y,z;

  // some Alloc Stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(tmp, double, n, "SVDdcmp::tmp");
#endif
#ifdef HAVE_MDMALLOC
  tmp = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(tmp[0]),0, sizeof(double)*n );
#endif

  // Copy of A into U
  for (int i = 0; i < m; ++i)
    {
      for (int j = 0; j < n; ++j)
        {
          U[i][j] = A[i][j];
        }
    }

  g     = 0.0;
  scale = 0.0;
  anorm = 0.0;
  for (int i=0; i < n; ++i)
    {
      l = i+1;
      tmp[i] = scale*g;
      g     = 0.0;
      s     = 0.0;
      scale =0.0;
      if (i < m)
        {
          for (int k=i; k < m; ++k)
            {
              scale += fabs(U[k][i]);
            }
          if (scale != 0.0)
            {
              for (int k=i; k < m; ++k)
                {
                  U[k][i] /= scale;
                  s += pow(U[k][i],2.0);
                }
              f = U[i][i];
              g = -sign(sqrt(s),f);
              h = f*g-s;
              U[i][i] = f - g;
              for (int j=l; j<n; ++j)
                {
                  s = 0.0;
                  for (int k=i; k< m; ++k)
                    {
                      s += U[k][i]*U[k][j];
                    }
                  f=s/h;
                  for (int k=i; k< m; ++k)
                    {
                      U[k][j] += f*U[k][i];
                    }
                }
              for (int k=i; k< m; ++k)
                {
                  U[k][i] *= scale;
                }
              // Fin boucle sur scale != 0.0
            }
          // fin boucle sur i <= m;
        }
      w[i] = scale*g;
      g = 0.0;
      s=0.0;
      scale=0.0;
      if ( (i < m) && (i!=n-1) )
        {
          for (int k=l; k <n; ++k)
            {
              scale += fabs(U[i][k]);
            }
          if (scale != 0.0)
            {
              for (int k=l; k<n; ++k)
                {
                  U[i][k] /= scale;
                  s += pow(U[i][k],2.0);
                }
              f = U[i][l];
              g = -1*sign(sqrt(s),f);
              h=f*g-s;
              U[i][l]=f-g;
              for (int k=l; k<n; ++k)
                {
                  tmp[k]=U[i][k]/h;
                }
              for (int j=l; j<m; ++j)
                {
                  s=0.0;
                  for (int k=l; k<n; ++k)
                    {
                      s += U[j][k]*U[i][k];
                    }
                  for (int k=l; k<n; ++k)
                    {
                      U[j][k] += s*tmp[k];
                    }
                }
              for (int k=l; k<n; ++k)
                {
                  U[i][k] *= scale;
                }
              //fin if sur scale
            }
          //fin if sur i, m,n
        }
      anorm = (std::max)( anorm, (fabs(w[i])+fabs(tmp[i])) );
    }
  for (int i=n-1; i >=0; --i)
    {
      if (i < n - 1)
        {
          if (g!=0.0)
            {
              for (int j=l; j <n; ++j)
                {
                  V[j][i] = (U[i][j]/U[i][l])/g;
                }
              for (int j=l; j <n; ++j)
                {
                  s=0.0;
                  for (int k=l; k <n; ++k)
                    {
                      s+=U[i][k]*V[k][j];
                    }
                  for (int k=l; k <n; ++k)
                    {
                      V[k][j] += s*V[k][i];
                    }
                }
            }
          for (int j=l; j<n; ++j)
            {
              V[i][j]=0.0;
              V[j][i]=0.0;
            }
        }
      V[i][i] = 1.0;
      g=tmp[i];
      l=i;
    }
  for (int i=(std::min)(m,n)-1; i>=0; --i)
    {
      l=i+1;
      g=w[i];
      for (int j=l; j<n; ++j)
        {
          U[i][j]=0.0;
        }
      if ( g!=0.0)
        {
          g = 1.0/g;
          for (int j=l; j<n; ++j)
            {
              s=0.0;
              for (int k=l; k<m; ++k)
                {
                  s += U[k][i]*U[k][j];
                }
              f  = (s/U[i][i])*g;
              for (int k=i; k<m; ++k)
                {
                  U[k][j]+=f*U[k][i];
                }
            }
          for (int j=i; j<m; ++j)
            {
              U[j][i] *=g;
            }
        }
      else
        {
          for (int j=i; j<m; ++j)
            {
              U[j][i]=0.0;
            }
        }
      U[i][i] += 1.0;
    }
  for (int k=n-1; k>=0; --k)
    {
      for (int its=0; its< itmax; ++its)
        {
          flag = true;
          for (l=k; l>=0; --l)
            {
              nm=l-1;
              if ( (fabs(tmp[l])+anorm)==anorm )
                {
                  flag = false;
                  break;
                }
              if ( (fabs(w[nm])+anorm)==anorm )
                {
                  break;
                }
            }
          if (flag)
            {
              c=0.0;
              s=1.0;
              for (int i=l; i<=k; ++i)
                {
                  f = s*tmp[i];
                  tmp[i]=c*tmp[i];
                  if ( (fabs(f)+anorm)==anorm )
                    {
                      break;
                    }
                  g=w[i];
                  h=pythag(f,g);
                  w[i]=h;
                  h=1.0/h;
                  c=g*h;
                  s=-f*h;
                  for (int j=0; j<m; ++j)
                    {
                      y=U[j][nm];
                      z=U[j][i];
                      U[j][nm]=y*c + z*s;
                      U[j][i]=z*c - y*s;
                    }
                }
            }
          z = w[k];
          if (l==k)
            {
              if ( z < 0.0)
                {
                  w[k] = -z;
                  for (int j=0; j<n; ++j)
                    {
                      V[j][k] = - V[j][k];
                    }
                }
              break;
            }
          if (its==itmax)
            {
              std::cout << "No convergence in SVDdcmp after " << itmax << " iterations\n";
            }
          x = w[l];
          nm = k-1;
          y=w[nm];
          g=tmp[nm];
          h=tmp[k];
          f = ((y-z)*(y+z) +(g-h)*(g+h))/(2.0*h*g);
          g = pythag(f,1.0);
          f = ((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x;
          c = 1.0;
          s = 1.0;
          for (int j=l; j<=nm; ++j)
            {
              g=tmp[j+1];
              y=w[j+1];
              h=s*g;
              g=c*g;
              z=pythag(f,h);
              tmp[j]=z;
              c=f/z;
              s=h/z;
              f=x*c+g*s;
              g = g*c-x*s;
              h=y*s;
              y*=c;
              for (int i=0; i<n; ++i)
                {
                  x=V[i][j];
                  z=V[i][j+1];
                  V[i][j]=x*c+z*s;
                  V[i][j+1]=z*c-x*s;
                }
              z = pythag(f,h);
              w[j]=z;
              if ( z!=0.0)
                {
                  z=1.0/z;
                  c=f*z;
                  s=h*z;
                }
              f=c*g+s*y;
              x=c*y-s*g;
              for (int i=0; i<m; ++i)
                {
                  y=U[i][j];
                  z=U[i][j+1];
                  U[i][j]=y*c+z*s;
                  U[i][j+1]=z*c-y*s;
                }
            }
          tmp[l]=0.0;
          tmp[k]=f;
          w[k]=x;
        }

    }
  free(tmp);
}


double* SVDsolve(double** & A, double* & b, int m, int n)
{
  // Solves Ax = b, using a QR algorithm
  double* x;

  // locals
  double** U = NULL;
  double** V = NULL;
  double*     w = NULL;

  // more locals
  double* tmp;
  double s;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(U, double*, m, "SVDsolve::U_l");
  for (int i=0; i < m; ++i)
    {
      GN_CALLOC(U[i], double, n, "SVDsolve::U_c");
    }
  GN_CALLOC(V, double*, n, "SVDsolve::V_l");
  for (int i=0; i < m; ++i)
    {
      GN_CALLOC(V[i], double, n, "SVDsolve::V_c");
    }
  GN_CALLOC(w, double, n, "SVDsolve::w");
  GN_CALLOC(x, double, n, "SVDsolve::x");
  GN_CALLOC(tmp, double, n, "SVDsolve::tmp");
#endif
#ifdef HAVE_MDMALLOC
  U     = (double**)vmdmalloc(sizeof(double), 2, m, n);
  V     = (double**)vmdmalloc(sizeof(double), 2, n, n);
  w     = (double*)vmdmalloc(sizeof(double), 1, n);
  x     = (double*)vmdmalloc(sizeof(double), 1, n);
  tmp     = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(U[0][0]),0, sizeof(double)*m*n );
  memset( &(V[0][0]),0, sizeof(double)*n*n );
  memset( &(w[0]),0, sizeof(double)*n );
  memset( &(x[0]),0, sizeof(double)*n );
  memset( &(tmp[0]),0, sizeof(double)*n );
#endif

  SVDdcmp(A, m, n, U, w, V);
#ifdef RETD_DEBUG
  for (int i=0; i<m; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          std::cout << " U["<<i<<"]["<<j<<"]="<<U[i][j];
        }
      std::cout << "\n";
    }
  for (int i=0; i<n; ++i)
    {
      for (int j=0; j<n; ++j)
        {
          std::cout << " V["<<i<<"]["<<j<<"]="<<V[i][j];
        }
      std::cout << "\n";
      std::cout << "w["<<i<<"]="<<w[i]<<"\n";
    }
  if ( m==n )
    {
      double** rr=NULL;
      rr = matdiagtranspose(U,w,V,n);
      for (int i=0; i<n; ++i)
        {
          for (int j=0; j<n; ++j)
            {
              std::cout << " rr["<<i<<"]["<<j<<"]="<<rr[i][j];
            }
          std::cout<<"\n";
        }
#ifndef HAVE_MDMALLOC
      for (int i=0; i<n; ++i)
        {
          free(rr[i]);
        }
#endif
      free(rr);
    }
#endif

  for (int j=0; j<n; ++j)
    {
      s=0.0;
      if (w[j]!=0.0)
        {
          for (int i=0; i<m; ++i)
            {
              s += U[i][j]*b[i];
            }
          s /= w[j];
        }
      tmp[j] = s;
    }
  for (int j=0; j < n; ++j)
    {
      s=0.0;
      for (int k=0; k < n; ++k)
        {
          s += V[j][k]*tmp[k];
        }
      x[j]=s;
    }

  // some Clean stuff
#ifndef HAVE_MDMALLOC
  for (int i=0; i<m; ++i)
    {
      free(U[i]);
    }
  for (int i=0; i<n; ++i)
    {
      free(V[i]);
    }
#endif
  free(U);
  free(V);
  free(w);
  free(tmp);

  return x;
}

void svdcmp( blitz::Array<double, 2> & a , blitz::Array<double,1> & w , blitz::Array<double,2> &v)
{
  bool flag;
  int i,its,j,jj,k,l,nm;
  double anorm,c,f,g,h,s,scale,x,y,z;

  int m=a.rows();
  int n=a.columns();
  blitz::Array<double,1>  rv1(n);
  g=scale=anorm=0.0;
  for (i=0; i<n; i++)
    {
      l=i+2;
      rv1(i)=scale*g;
      g=s=scale=0.0;
      if (i < m)
        {
          for (k=i; k<m; k++) scale += fabs(a(k,i));
          if (scale != 0.0)
            {
              for (k=i; k<m; k++)
                {
                  a(k,i) /= scale;
                  s += a(k,i)*a(k,i);
                }
              f=a(i,i);
              g = -sign(sqrt(s),f);
              h=f*g-s;
              a(i,i)=f-g;
              for (j=l-1; j<n; j++)
                {
                  for (s=0.0,k=i; k<m; k++) s += a(k,i)*a(k,j);
                  f=s/h;
                  for (k=i; k<m; k++) a(k,j) += f*a(k,i);
                }
              for (k=i; k<m; k++) a(k,i) *= scale;
            }
        }
      w(i)=scale *g;
      g=s=scale=0.0;
      if (i+1 <= m && i != n)
        {
          for (k=l-1; k<n; k++) scale += fabs(a(i,k));
          if (scale != 0.0)
            {
              for (k=l-1; k<n; k++)
                {
                  a(i,k) /= scale;
                  s += a(i,k)*a(i,k);
                }
              f=a(i,l-1);
              g = -sign(sqrt(s),f);
              h=f*g-s;
              a(i,l-1)=f-g;
              for (k=l-1; k<n; k++) rv1(k)=a(i,k)/h;
              for (j=l-1; j<m; j++)
                {
                  for (s=0.0,k=l-1; k<n; k++) s += a(j,k)*a(i,k);
                  for (k=l-1; k<n; k++) a(j,k) += s*rv1(k);
                }
              for (k=l-1; k<n; k++) a(i,k) *= scale;
            }
        }
      anorm=(std::max)(anorm,(fabs(w(i))+fabs(rv1(i))));
    }
  for (i=n-1; i>=0; i--)
    {
      if (i < n-1)
        {
          if (g != 0.0)
            {
              for (j=l; j<n; j++)
                v(j,i)=(a(i,j)/a(i,l))/g;
              for (j=l; j<n; j++)
                {
                  for (s=0.0,k=l; k<n; k++) s += a(i,k)*v(k,j);
                  for (k=l; k<n; k++) v(k,j) += s*v(k,i);
                }
            }
          for (j=l; j<n; j++) v(i,j)=v(j,i)=0.0;
        }
      v(i,i)=1.0;
      g=rv1(i);
      l=i;
    }
  for (i=(std::min)(m,n)-1; i>=0; i--)
    {
      l=i+1;
      g=w(i);
      for (j=l; j<n; j++) a(i,j)=0.0;
      if (g != 0.0)
        {
          g=1.0/g;
          for (j=l; j<n; j++)
            {
              for (s=0.0,k=l; k<m; k++) s += a(k,i)*a(k,j);
              f=(s/a(i,i))*g;
              for (k=i; k<m; k++) a(k,j) += f*a(k,i);
            }
          for (j=i; j<m; j++) a(j,i) *= g;
        }
      else for (j=i; j<m; j++) a(j,i)=0.0;
      ++a(i,i);
    }
  for (k=n-1; k>=0; k--)
    {
      for (its=0; its<30; its++)
        {
          flag=true;
          for (l=k; l>=0; l--)
            {
              nm=l-1;
              if (fabs(rv1(l))+anorm == anorm)
                {
                  flag=false;
                  break;
                }
              if (fabs(w(nm))+anorm == anorm) break;
            }
          if (flag)
            {
              c=0.0;
              s=1.0;
              for (i=l-1; i<k+1; i++)
                {
                  f=s*rv1(i);
                  rv1(i)=c*rv1(i);
                  if (fabs(f)+anorm == anorm) break;
                  g=w(i);
                  h=sqrt(f*f+g*g) ;
                  w(i)=h;
                  h=1.0/h;
                  c=g*h;
                  s = -f*h;
                  for (j=0; j<m; j++)
                    {
                      y=a(j,nm);
                      z=a(j,i);
                      a(j,nm)=y*c+z*s;
                      a(j,i)=z*c-y*s;
                    }
                }
            }
          z=w(k);
          if (l == k)
            {
              if (z < 0.0)
                {
                  w(k) = -z;
                  for (j=0; j<n; j++) v(j,k) = -v(j,k);
                }
              break;
            }
          if (its == 29)
            {
              std::cout << "no convergence in 30 svdcmp iterations" << std::endl ;
              abort() ;
            }
          x=w(l);
          nm=k-1;
          y=w(nm);
          g=rv1(nm);
          h=rv1(k);
          f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
          g=sqrt(f*f+1.);
          f=((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x;
          c=s=1.0;
          for (j=l; j<=nm; j++)
            {
              i=j+1;
              g=rv1(i);
              y=w(i);
              h=s*g;
              g=c*g;
              z=sqrt(f*f+h*h);
              rv1(j)=z;
              c=f/z;
              s=h/z;
              f=x*c+g*s;
              g=g*c-x*s;
              h=y*s;
              y *= c;
              for (jj=0; jj<n; jj++)
                {
                  x=v(jj,j);
                  z=v(jj,i);
                  v(jj,j)=x*c+z*s;
                  v(jj,i)=z*c-x*s;
                }
              z=sqrt(f*f+ h*h);
              w(j)=z;
              if (z)
                {
                  z=1.0/z;
                  c=f*z;
                  s=h*z;
                }
              f=c*g+s*y;
              x=c*y-s*g;
              for (jj=0; jj<m; jj++)
                {
                  y=a(jj,j);
                  z=a(jj,i);
                  a(jj,j)=y*c+z*s;
                  a(jj,i)=z*c-y*s;
                }
            }
          rv1(l)=0.0;
          rv1(k)=f;
          w(k)=x;
        }
    }
}

void svdksb(blitz::Array<double,2> & u, blitz::Array< double,1> & w, blitz::Array<double,2> & v, blitz::Array<double,1> & b, blitz::Array<double,1> & x)
{
  int n= w.size() ;
  int m = b.size() ;

  blitz::Array<double,1> tmp(n) ;

  // calculate Ut B
  for (int j=0; j<n; j++)
    {
      double s=0.0;
      if (w(j) != 0.0)
        {
          for (int i=0; i<m; i++) s += u(i,j)*b(i);
          s /= w(j);
        }
      tmp(j)=s;
    }
  for (int j=0; j<n; j++)
    {
      double s=0.0;
      for (int jj=0; jj<n; jj++) s += v(j,jj)*tmp(jj);
      x(j)=s;
    }
}

bool is_diagMat(double** & A, int n)
{
  bool res = true;
  for (int i=0; i < n; ++i)
    {
      for (int j=0; j < n; ++j)
        {
          if ( (i != j) && (fabs(A[i][j]) > 1e-12) )
            {
              res = false;
              break;
            }
        }
      if (!res)
        {
          break;
        }
    }
  return res;
}

double* Diagsolve(double** const A, double* const b, int n)
{
  // Solves Ax = b, supposing A diagonal
  double* x;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(x, double, n, "Diagsolve::x");
#endif
#ifdef HAVE_MDMALLOC
  x  = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(x[0]),0, sizeof(double)*n );
#endif

  for (int i=0; i < n; ++i)
    {
      if ( fabs(A[i][i]) > 0.0 )
        {
          x[i] = b[i]/A[i][i];
        }
      else
        {
          if (fabs(b[i]) > 1E-13)
            {
              std::cout << "b["<<i<<"]="<<b[i]<<"\n";
              std::cout << "[DiagSolve] : Unsolvable system\n";
              exit(false);
            }
          else
            {
              x[i] = 0.0;
            }
        }
    }
  return x;
}

double** Choldcmp( double** A, int n, bool& isOk)
{
  double** C=NULL;
  // A = C C^T
  isOk = true;

  // locals
  int i,k;
  double* p = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(p, double, n, "Cholsolve::p");
  GN_CALLOC(C, double*, n, "Cholsolve::C_l");
  for (i=0; i < n; ++i)
    {
      GN_CALLOC(C[i], double, n, "Cholsolve::C_c");
    }
#else
  C  = (double**)vmdmalloc(sizeof(double), 2, n,n);
  p  = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(C[0][0]),0, sizeof(double)*n*n );
  memset( &(p[0]),0, sizeof(double)*n );
#endif

  // On fait un copie de A et on bidouille
  for (i=0; i < n; ++i)
    {
      for (int j=0; j < n; ++j)
        {
          C[i][j] = A[i][j];
        }
    }

  //Calcul de p : la diagonale de A dans le Choleski
  for (i=0 ; i< n ; ++i)
    for (int j=i; j< n ; ++j)
      {
        double sum2 = C[i][j];
        for (k =i-1; k>=0 ; --k)
          {
            sum2 -= C[i][k]*C[j][k];
          }
        if (i==j)
          {
            // Checking if Matrix is PSD, if not continue anyways but keep track of this event in isOk
            if ( sum2 <= 0.0 )
              {
                isOk = false;
                sum2 = 1e-6; //This is a Lame patch
              }
            //assert(sum2>0.);
            p[i] =sqrt(sum2) ;
          }
        else
          {
            C[j][i]=sum2/p[i];
          }
      }

  // On ecrase la sur-diagonale de C et on remplace la diagonale avec p
  for (i =0; i < n; ++i)
    {
      for (int j=i; j < n; ++j)
        {
          C[i][j]=0.0;
        }
      C[i][i] = p[i];
    }

#ifdef RETD_DEBUG
  //show A
  for (i=0; i < n; ++i)
    {
      for (int j=0; j < n; ++j)
        {
          std::cout << C[i][j]<< " ";
        }
      std::cout << "\n";
    }
  for (i=0; i < n; ++i)
    {
      std::cout << "p["<<i<<"]="<<p[i]<<"\n";
    }
#endif

  free(p);

  return C;
}

double* Cholsolve( double** & A , double* & b, int n)
{
  //  Resout Ax=b pour a sym. def. pos. A
  double* x = NULL;

  // locals
  int i,k;
  double* p = NULL;

  // Alloc stuff
#ifndef HAVE_MDMALLOC
  GN_CALLOC(x, double, n, "Cholsolve::x");
  GN_CALLOC(p, double, n, "Cholsolve::p");
#endif
#ifdef HAVE_MDMALLOC
  x  = (double*)vmdmalloc(sizeof(double), 1, n);
  p  = (double*)vmdmalloc(sizeof(double), 1, n);
  memset( &(x[0]),0, sizeof(double)*n );
  memset( &(p[0]),0, sizeof(double)*n );
#endif

  //Calcul de p : la diagonale de A dans le Choleski
  for (i=0 ; i< n ; ++i)
    for (int j=i; j< n ; ++j)
      {
        double sum2 = A[i][j];
        for (k =i-1; k>=0 ; --k)
          {
            sum2 -= A[i][k]*A[j][k];
          }
        if (i==j)
          {
            assert(sum2>0.);
            p[i] =sqrt(sum2) ;
          }
        else
          {
            A[j][i]=sum2/p[i];
          }
      }

#ifdef RETD_DEBUG
  //show A
  for (i=0; i < n; ++i)
    {
      for (int j=0; j < n; ++j)
        {
          std::cout << A[i][j]<< " ";
        }
      std::cout << "\n";
    }
  for (i=0; i < n; ++i)
    {
      std::cout << "p["<<i<<"]="<<p[i]<<"\n";
    }
#endif

  double sum;
  for(i=0; i<n; ++i)
    {
      for (sum=b[i],k=i-1; k>=0; --k) sum -= A[i][k]*x[k];
      x[i]=sum/p[i];
    }
  for (i=n-1; i>=0; --i)
    {
      for(sum=x[i],k=i+1; k<n; ++k) sum -= A[k][i]*x[k];
      x[i]=sum/p[i];
    }

  free(p);
  return x;
}

blitz::Array<double,2> Choldcmp( blitz::Array<double,2> & A, bool& isOk)
{
  int n = A.extent(0);
  if ( n != A.extent(1) )
    {
      std::cout << "[Choldcmp] : Error, called choleski decomposition on a non-square matrix !\n";
      exit(false);
    }
  // Alloc stuff
  blitz::Array<double,2> C(n,n);
  blitz::Array<double,1> p(n);

  // A = C C^T
  isOk = true;

  // locals
  int i,k;

  // On fait un copie de A et on bidouille
  C = A;
  /*for (i=0; i < n; ++i){
  	for (int j=0; j < n; ++j){
  		C(i,j) = A(i,j);
  	}
  }*/

  //Calcul de p : la diagonale de A dans le Choleski
  for (i=0 ; i< n ; ++i)
    for (int j=i; j< n ; ++j)
      {
        double sum2 = C(i,j);
        for (k =i-1; k>=0 ; --k)
          {
            sum2 -= C(i,k)*C(j,k);
          }
        if (i==j)
          {
            // Checking if Matrix is PSD, if not continue anyways but keep track of this event in isOk
            if ( sum2 <= 0.0 )
              {
                isOk = false;
                sum2 = 1e-6; //This is a Lame patch
              }
            //assert(sum2>0.);
            p(i) =sqrt(sum2) ;
          }
        else
          {
            C(j,i)=sum2/p(i);
          }
      }

  // On ecrase la sur-diagonale de C et on remplace la diagonale avec p
  for (i =0; i < n; ++i)
    {
      for (int j=i; j < n; ++j)
        {
          C(i,j)=0.0;
        }
      C(i,i) = p(i);
    }

#ifdef RETD_DEBUG
  //show A
  for (i=0; i < n; ++i)
    {
      for (int j=0; j < n; ++j)
        {
          std::cout << C(i,j)<< " ";
        }
      std::cout << "\n";
    }
  for (i=0; i < n; ++i)
    {
      std::cout << "p["<<i<<"]="<<p(i)<<"\n";
    }
#endif

  p.free();

  return C;
}
